#include <bits/types.h>

#include <cmath>               // C++ 数学库
#include <ctime>               // 程序计时
#include <eigen3/Eigen/Dense>  // Eigen矩阵运算库，用于矩阵计算
#include <fstream>             // 文件输入输出流
#include <iostream>            // 输入输出流
#include <limits>  // 决定了各种变量类型的各种属性，例如一个无符号可以存储的最大值是 255
#include <string>  // 处理字符串
#include <vector>  // STL 动态数组容器

using namespace std;
using namespace Eigen;
double generateGaussianNoise(double miu,
                             double sigma);  //随机高斯分布数列生成器函数

int main(void) {
  clock_t start, stop;
  double duration;  //记录被测函数运行时间,以s为单位
  start = clock();  //开始计时
  ofstream fout("..//position.txt");  //""中是txt文件路径，注意：路径要用//隔开

  const double delta_t = 0.1;  //控制周期，100ms
  const int num = 100;         //迭代次数
  const double acc = 10;       //加速度，ft/m

  MatrixXd A(2, 2);
  A << 1, delta_t, 0, 1;

  MatrixXd B(2, 1);
  B << (pow(delta_t, 2) / 2), delta_t;

  MatrixXd H(1, 2);  //测量的是小车的位移，速度为0
  H << 1, 0;

  MatrixXd Q(2, 2);  //过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，
                     //且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
  Q << 0, 0, 0, 0.01;

  MatrixXd R(
      1,
      1);  //观测噪声协方差，测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
  R << 10;

  vector<double> time(100, 0);  // 一维 vector --> time,大小为100，初值为0
  MatrixXd X_real(2, 1);        // 真实状态
  MatrixXd X_rand(1, 1);        // 高斯分布随机数测量误差
  MatrixXd X_evlt = MatrixXd::Constant(2, 1, 0);  // 系数为0，即初始化为0
  MatrixXd X_pdct = MatrixXd::Constant(2, 1, 0);  // 系数为0，即初始化为0
  MatrixXd Z_meas = MatrixXd::Constant(1, 1, 0);  // 系数为0，即初始化为0
  MatrixXd Pk = MatrixXd::Constant(2, 2, 0);    // 系数为0，即初始化为0
  MatrixXd Pk_p = MatrixXd::Constant(2, 2, 0);  // 系数为0，即初始化为0
  MatrixXd K = MatrixXd::Constant(2, 1, 0);     // 系数为0，即初始化为0
  vector<MatrixXd> x_real;                      // 真实状态
  vector<MatrixXd> x_rand;  // 高斯分布随机数测量误差
  vector<MatrixXd> x_evlt;  // 状态估计值
  vector<MatrixXd> x_pdct;  // 状态预测值
  vector<MatrixXd> z_meas;  // 状态测量值
  vector<MatrixXd> pk;      // 估计状态和真实状态的协方差矩阵
  vector<MatrixXd> pk_p;    // 预测状态与真实状态的协方差矩阵
  vector<MatrixXd> k;       // 卡尔曼增益

  //初始化，均为0
  x_evlt.push_back(X_evlt);
  x_pdct.push_back(X_pdct);
  z_meas.push_back(Z_meas);
  pk.push_back(Pk);
  pk_p.push_back(Pk_p);
  k.push_back(K);

  //高斯分布的随机数测量误差，真实的位移值
  for (int i = 0; i < 100; ++i) {
    time[i] = i * delta_t;
    X_rand(0, 0) = generateGaussianNoise(0, sqrt(10));
    x_rand.push_back(X_rand);
    X_real(0, 0) = 0.5 * acc * pow(time[i], 2);
    X_real(1, 0) = 0;
    x_real.push_back(X_real);
  }
  //开始迭代
  cout << "含噪声测量"
       << "  "
       << "状态预测"
       << "  "
       << "最优估计"
       << "  "
       << "真实值" << endl;
  for (int i = 1; i < num; ++i) {
    //预测值
    X_pdct = A * x_evlt[i - 1] + B * acc;
    x_pdct.push_back(X_pdct);
    //预测状态与真实状态的协方差矩阵
    Pk_p = A * pk[i - 1] * A.transpose() + Q;  // a.transpose() 矩阵转置
    pk_p.push_back(Pk_p);
    //卡尔曼增益矩阵 K:2x1
    MatrixXd tmp(1, 1);
    tmp = H * pk_p[i] * H.transpose() + R;
    K = pk_p[i] * H.transpose() * tmp.inverse();  // a.inverse() 矩阵求逆
    k.push_back(K);
    //测量值
    Z_meas = H * x_real[i] + x_rand[i];
    z_meas.push_back(Z_meas);
    //估计值
    X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
    x_evlt.push_back(X_evlt);
    //估计状态和真实状态的协方差矩阵
    Pk = (MatrixXd::Identity(2, 2) - k[i] * H) *
         pk_p[i];  // MatrixXd::Identity(2, 2)---2阶单位矩阵
    pk.push_back(Pk);

    cout << z_meas[i] << "  " << x_pdct[i](0, 0) << "  " << x_evlt[i](0, 0)
         << "  " << x_real[i](0, 0) << endl;
    fout << z_meas[i] << "  " << x_pdct[i](0, 0) << "  " << x_evlt[i](0, 0)
         << "  " << x_real[i](0, 0) << endl;
  }
  fout.close();
  /* Matlab 画图
  a = dlmread('result.txt');
  plot(a)*/
  cout << endl;
  stop = clock();  //结束计时
  duration = ((double)(stop - start)) /
             1000;  // CLK_TCK = 1000, 6x86_64-w64-mingw32-gcc.exe编译器
  cout << "程序用时" << duration << endl;

  return 0;
}

//生成高斯分布随机数的函数
double generateGaussianNoise(double miu, double sigma) {
  const double epsilon = std::numeric_limits<double>::min();  // <limits>
  const double two_pi = 2.0 * 3.14159265358979323846;

  static double z0, z1;
  static bool generate;
  generate = !generate;

  if (!generate) return z1 * sigma + miu;

  double u1, u2;
  do {
    u1 = rand() * (1.0 / RAND_MAX);  // <limits>
    u2 = rand() * (1.0 / RAND_MAX);
  } while (u1 <= epsilon);

  z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
  z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
  return z0 * sigma + miu;
}
